#ifndef NAV_CONTROLLER__NAV_CONTROLLER_HPP_
#define NAV_CONTROLLER__NAV_CONTROLLER_HPP_

#include <string>
#include <vector>
#include <memory>

#include "nav2_core/controller.hpp"
#include "rclcpp/rclcpp.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
// 自定义
#include "nav_controller/sensor/laser_util.hpp"
#include "rviz_util_pkg/rviz_marker.hpp"

namespace nav_controller
{

  class CustomController : public nav2_core::Controller
  {
  public:
    CustomController() = default;
    ~CustomController() override = default;

    void configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
        std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
        const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

    void cleanup() override;
    void activate() override;
    void deactivate() override;
    /**
     * 设置速度限制
     * @param speed_limit 限速值
     * @param percentage  是否为百分比形式的速度限制
     */
    void setSpeedLimit(const double &speed_limit, const bool &percentage) override;
    /**
     * @param pose 机器人当前位姿
     * @param velocity 机器人当前速度,包括线速度、角速度
     * @param goal_checker 机器人目标点检查器
     */
    geometry_msgs::msg::TwistStamped
    computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
                            const geometry_msgs::msg::Twist &velocity,
                            nav2_core::GoalChecker *goal_checker) override;
    /**
     * 设置全局路径计划
     */
    void setPlan(const nav_msgs::msg::Path &path) override;
    /**
     * @param robot_pose 机器人当前位姿
     * @return 裁剪后的全局路径  frame_id:robot_pose.header.frame_id = costmap_ros_->getBaseFrameID()
     */
    nav_msgs::msg::Path transformGlobalPlan(geometry_msgs::msg::PoseStamped robot_pose);

  protected:
    // 获取路径中距离当前点最近的点
    geometry_msgs::msg::PoseStamped
    getLookAheadPose(const geometry_msgs::msg::PoseStamped &current_pose,
                     const nav_msgs::msg::Path &global_plan_frame);
    // 计算角度差
    double calculateAngleDifference(const geometry_msgs::msg::PoseStamped &current_pose,
                                    const geometry_msgs::msg::PoseStamped &target_pose);

    rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
    std::shared_ptr<tf2_ros::Buffer> tf_;
    std::string plugin_name_;
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
    rclcpp::Logger logger_{rclcpp::get_logger("CustomController")};
    rclcpp::Clock::SharedPtr clock_;

    double desired_linear_vel_;
    double lookahead_dist_;
    double max_angular_vel_;
    double transform_tolerance_;

    nav_msgs::msg::Path global_plan_;
    std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_pub_;

    std::unique_ptr<nav_controller::LaserUtil> laser_util_;
    std::unique_ptr<rviz_util_pkg::RvizMarker> rviz_marker_;
  };

} // namespace nav_controller

#endif // NAV_CONTROLLER__NAV_CONTROLLER_HPP_